#!/usr/bin/python
## NAO-ROBOTICS FINAL PROJECT 
'''
	CS494 FALL 2011 
	DR. PARKER
	GROUP 3: 
		TYLER BURKLE
		TRUSHNA PATEL
		CURTIS TAYLOR
'''
#

from naoqi import *
IP = '10.26.210.XX' #robot's IP
PORT = 9559
speech	= ALProxy("ALTextToSpeech", IP, PORT) 
mp		= ALProxy("ALMotion", IP, PORT) 
b		= ALProxy("ALBehaviorManager", IP, PORT) 
sonar	= ALProxy("ALSonar", IP, PORT) 
mem		= ALProxy('ALMemory', IP, PORT)
red		= ALProxy('ALRedBallDetection', IP, PORT)

sonar.subscribe("SonarTest", 500, 1.0) 

period = 500
red.subscribe("RedBallDetected", period, 0.0)

#Stand up
speech.post.say("Standing Up") 
mp.setStiffnesses('Body', 1)
b.runBehavior('standUp')
mp.setWalkArmsEnable(True,True)

#Stop robot if head is tapped
headMiddle = 0
time.sleep(2)

while headMiddle == 0:
	memValue = "RedBallDetected"

	#should robot be turning while below loop is running?

	for i in range(0, 20):
		time.sleep(0.5)
		val = mem.getData(memValue, 0)
		
		#Valid output: a list with three fields
		#[ [time_info], [ball_info], [camera_info] ]
		if(val and isinstance(val, list) and len(val) == 3):
			#RED BALL DETECTED, GET INFO!
			
			#First field = time_info
			timeInfo = val[0]

			#Second field = ball_info
			ballInfo = val[1]

			#Third field = camera_info
			cameraInfo = val[2]

			#Find where red ball is located
			#[u, v, centerX, centerY, radius, score]
			for size in ballInfo:
				#First field = u
				u = size[0]

				#Second field = v
				v = size[1]

				#centerX and centerY are angular coordinates in camera angles
				#Third field = centerX
				centerX = size[2]

				#Fourth field = centerY
				centerY = size[3]

				#Ball radius in pixels
				#Fifth field = radius
				radius = size[4]

				#Sixth field = score
				#If red ball is detected, then score = 1, else 0
				score = size[5]

			print "x: %f" % (centerX)
			print "y: %f" % (centerY)
			print "radius: %f" % (radius)
			print "score: %f" % (score)

	#Move robot towards red ball
	#x and y values should be in meters
	b.walkTo(centerX, centerY, some_theta)

	headMiddle = mem.getData('Device/SubDeviceList/Head/Touch/Middle/Sensor/Value', 0)

#NEED a stop all action before trying to sit down

#Unsubscribe ALRedBallDetection
red.unsubscribe("RedBallDetected")

#Once robot is done collecting red balls, make it sit down
speech.post.say("Sitting down")
b.runBehavior("sitDown")
mp.setStiffnesses('Body', 0)

